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(54)Title: MOVEMENT CONTROL METHOD FOR TEACHING POSITION OF ROBOT 



(57) Abstract 

A mark member (Al) is stuck onto a work 
(3) with a certain relation with a teaching desired 
position on the work (3). The movement of the 
robot is autonomously controlled and the position 
teaching is performed in accordance with the 
present positions of the robot hand at the 
respective timings of the robot movement, the 
positions on a camera coordinate system of the 
mark member at the respective timings of the 
robot movement and the postures of the robot 
viewed from the camera coordinate system, the 
position on the camera coordinate system of the 
mark member (Al) when the robot reaches the 
teaching desired position and the posture of the 
robot viewed from the camera coordinate system, 
and the position on the camera coordinate system 
and the posture of the robot viewed from the 
robot hand. 
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<rM>-a<el>=t-lRM i <d-l> ••• (18) 

dCTx RM" 1 tmlfr^nti-fftn-ZlbZ Z i £ t , (19) J5<fi£3: 



< m > + a < e 1 > = tl R M <dl > 

,-1 



CRM" 



< d i >) 



(RM 



-1 



< d j >) =<di > ♦ < d j > 
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(i.j— 2.-1.0.1.2) • • • (19) 

CO (19) Sti. (5) ~ (8) 5$&l>' (16) ~ (18) i*ffiV5t, from 
ft£ (20) ~ (23) WmttlZo 
II < r II > || 

2 
2 
2 



< rll > 

< r II > 

< r M > 



2 =t0 2 



+ 2 a r Mx + a 2 = t l 2 
- 2 a r Hx+ a 2 = t -I 2 



+ a r Mx = tO t 1 <5 0. 1 

<rM >|| 2 - ar Hx= t 0 t -15 0.-1 
,2^ , , n ,2 



(20) 
(21a ) 
(21b ) 
(22a ) 
(22b ) 
(23) 



5 o. r< l . 5 o. -r < l 

:n&©55fr&*# (3) . (9) (22 a) . (22 b ) * t> „ II < r K > 

I! 2 + a r Mxi a <5 0,K II < r 11 > II 2 - a r Mxi a 5 0. -1 i IsjW^JC 
tt Sfl:SI L TSltntf, (24a) ~ (25b) i<ilbn$. 



<rM >|| 2 + a r Mx= a a +/v7 f My 2 + r M2 2 



< rM > || * - a rMx= a a 



- sJ r M v 2 + r Hz2 



• • (2 4 a) 

• • (2 4 b) 



<1U 



def 



50,1 



a + 



/J 1 - 5 0. 1 2 



• • • (2 5 a) 



def 

a - = 



5 0.-1 



/7 1-80.-1 5 



• (2 5 b) 



5£ (24a) . (24b) liM ^B^T S £ . (26) 
II < r M > 



2 = (a 2 /9+ 2 ) / (1 + £- 2 ) 



(27) *<»&n*. 

• • • (26) 

r Mx= ( a 0 + /9 - ) / ( 1 + $ - £ ) • • • (27) 

(IU £ + . £- (ifc5* (28a) , (28b) Tt«$n5 fcOitSo 

0+=(a++a-)/2 •••(28 a) 

0--(a+-a-)/2 • • • (28b ) 

*LT> 55(20) (21a). (21 b ) \z&. (26) (27) ^ftALt^tt (3) . 

( 9 ) ^fti*> to. t 1 Rtf t -l*<2fc35 (29) (30 a). (30b) T*4 
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t 0 = a 



1 +P 



(2 9) 



t1 =a 



1 +g-r 
1 + p- 2 



(3 0 a) 



10 



t -1= a 



1 + a- 
1 + P 



(3 0 b) 



cn^fflUtli:, t2. t -2. < 1 M >&CKRM jMfccD.fc'J KJfcfe&ttSo 



15 



20 



t 1- t-1 51.-1 



t2 = 



1 1 



t1 61.2- t-1 8-1.2 

( t 1 51.2- t -1 5-1.2* 0 0D*g-&) 



t-1- t1 61,-1 



t-2 = 



t-1 



t-1 6-1.-2- t1 61.-2 

(t1 51.2- t-1 5-1.2=0(75*8^) 



< I H>= t 0< d0> 

<RM>= (< f 1 >< f 2 >< f 3 » 

1 



. • • (3 0 c 1 ) 



(1U < f 1 >- 



2 a 



C 1 1< d 1 >— t-1< d-1>) 



• (3 0 c 2) 

• C3 0 d) 

• (3 0 e) 

• (3 0 f ) 



25 



< f 2 >= -< 



• ( t 2< d 2> - 1 1< d t >) 
C 1 1 61.2— t-1 6-1.2#0©*&) 



1 



(t-2< d-2>- t -1< d-1» 

3 C 1 1 6 1.2- t-1 8-1.2- 0 



(30g) 



< f 3 > = < f 1 >x< f 2> 



• ♦ C3 0 h) 
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5. C©^»^ 

B*i£©&# (i4) ©tt*> mm<njjm& Q3) zm< 0 sis (13) 



*<fc. (31) , (32) #*#£tt*o 

C -1 T 1 _1 T3 C = P 1 P 3" 1 
C " 1 T2~ 1 T3 C = P 2 P 3 _1 
dC-T'&jS (33) ~ (37) ic&o tzmZ&z-Z'ate o • 



(31) 
(32) 



Rc 



I c 

1 



def 

= C 



• • • (3 3) 



R1 
t „ 



I 1 
1 



= T1 1 T3 



(3 4) 



R2 



I 2 

1 



def 



P1 P3 



-1 



(3 5) 



R3 



I 3 
1 



def , 

= T2 ' T3 



(3 6) 



R4 

t « 



I 4 
1 



def 

= P2P3 



t5i, (31) . (32) li^O^ (38) 

RC" 1 R 1 RC = R2 
,-1 



• • • (3 7) 

(4i) izfimztiz 



RC L R3 RC « R4 
(I-Rl) <1C > - < 1 1 >-RC < 1 2 > 
(I-R3) < 1 C > = < 1 3 >-RC < 1 4 > 



(38) 
(39) 
(40) 
(41) 



Vt-oT, RC E.tf< 1C Z> Z ki)<3&tt£Z>o ZZT. #§3: (38) . 

(39) EIlEft ?'Jfa<D*Bfl»R 1 ~R2 . R3 ~R4 &. V * ft h ©*B*EIW 

*«HHaE8HT*j*<£ *> tcRc 
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Ri (i-1.2.3.4 ) ©Ufe^ftS-^fr b;U&< vi > (i-1.2. 

3.4 ) t Lt> (42) . (43) *<»&ftSo < vi > (i=l. 2.3.4 ) 

It, (14) tfSS (34) . (36) * 6— Sttfjfc^feS - £#ttl*S, 

<vl> = RC<v2> • • • (42) 

5 <v3> = RC<v4> • • • (43) 

Ml:, (44) ©H«*ffl^titf, RC s c i*<lti*So 

(< v 1 >. <v3 >. < v 1 >x<v3 >) 
= RC (<v2 >, <v4 >. <v2 >x<v4 >) • • • (44) 
■fimX. (40) . (41) i:HLTti, (45) © J& iz l> t * t to ic L T, fi'h 

io 2 *ife*iffltntf< i c 6n* 0 



1 — RI 




< 1 1> 




< 1 2> 




< 1 C> = 




-RC 




1 -R3 




< 1 3> 




< 1 4> 



15 6. jffStl KJ:*»»*l»©teS:7n-©*fc 

( 1 ) ft*Htel=ft*£ o T, ±15 3 . TiftBJ L fc«fc 5 lc> 3o©&gCBl~ 
20 CB 3icn*'-v ^«I»S»$tt5o 

(2) §fi[IC B 1~CB 3f, R&V-t 3±©v-?«S*tMK 1 ©v-* 
ffi«&51M (1®) ©Bft&RftLx Ii«&JIgE2 OrtCItftlCi^-C^ 

(3) ±15 5. TK«Lfc7ArfiJ XAtcfto fc&g£li#4!iggB 2 OrtT* 
25 ff fJ y 7^«^C©x-^ (RC. <lC>©f*-^)«-iHH' IS tit" So 

(4) *yv + -^U^U-V3> *><%T Lfct>, n # y h * BI4 fc: ^ L & ■ 
P t i:^Sl$t, t-?MK1 iC*ft*#^»a&«3a«ttaS*flfcb<*i*-5c 

(5) v-^IISIZi ©B**S*»Lx ®«MSSH2 0 rt©li«ffiSlc«fc 
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(6)±E1. 2. 4. TKB^LfcT^=f U XACfifeofc«La*li««iaaE|l 

2 ort-eff ft^ftattBsjattasaaT*-^ cms ©■?-*) **ai • k» 

OISXt -y -fiStijEsm-** 9 ;L-iC«fc o THfT$ ft -So 

CK1) <d0 >. <dl >, <d-l>, <d2 >, < d-2>*It«I-p 
#*K MO *tf»f 

(K2] Ett*ftT^*Mg ©f-^i^f 7^KlT«J6fc<dO >. < 
dl >. <d-l>. <d2 >. < d-2>*&tf*3tt*M0 ££Jfc«L> Pi#«D 

t*l«tSJ^&, 7f »;7K3~K5*gftLT7,f";7Kl'\ii t Mg £ 
MO CO — Srft©»ffilil±«* CDT;l/=f 'J X Aj5<fi]ffl|Hi5feS c «*.M\ (46) 
-e^J^ft^ A L. A<£ ( £ ii + ^/httS3E«) T&fttfT^o-f-^T. 



def 

A = 



(Mo — Mg) 



"1 " 




2 


0 




+ 


0 






Oj 







(Mo— Mg) 



"0" 




1 




0 




0 





(Mo — Mg) 



0' 
0 

1 

0 



(Mo— Mg) 



"0" 




0 




0 




1 





(4 6) 



(K4) tpfllf^HT-ma L fcx- 9 (C Mg ©-r-*) i±ER«Lfc7 
AfJXiill^, ( 2 ) 3$©:&ia*tt-*U iS55^»j@SffiBT g 

■>7f Ai<iKi©^i<£ ©T-fcfttf^ £l±©Xf- ^ K 1 ~ K 5 O 1 t -f 

* ;UTtt^*affiK3BI«tt»KS**ifft £ BR tea a je * >■*©»** It 
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CK6] h*ffjt**r»I*»7t*. 
[1] «S 

H9-H1 4*»lHi:lBATSWt5o 0 9 I* ig AS ft US © IS & Pg <Z> F<3 P R 

1 ~ p r 3 zmnt z> fc*>® bt* k> > m 1 o \tm k> pr4~pr6£i& 

B^-rs^toOElT-*5o Ztz, Bllli, &7rsftnom®.V£Oft T H 1 ~T H 3 
fcS&ajl*-* fc»©Bk El 1 2 !i©PgT H 4 . P R 5 £g&BJ3-r* /cttcDglT* S 0 

[2] *t4!&ftl£ (tt**Hffilliy*#S8*8!7*-*©fl*»±:&> 704-+ 'J * 
U - > a > ) 

(PR1) *f 75>i?fiI2 aT'fta$n5o^9 hOffti:, fj / ^ 2 
WzMtLX^ K2 2 (H2 «C«Lfcl/-fJIM6 OOJRMtt'W K) 

i#JW , y-;U2 5*ffi0ft»t*« U-*^* K2 2 iiii ttHft«l«i 2 3 L 
IffiO ft ft 6 ttT* »K b-f f-A2 4 © &ft #13 £ IS til* 5 J; 5 tt o T 
<^*o J>JW*y - ^ 2 5 ©5feJ»ftiE©^B3 2 7 L TCP (2b) 

Httv-? 2 6 & nt^« e fiL, :0?-?26 lillfflO fcOT 

*5Ci icaftf* So H**ftTl> v-V^y K22*» 

? 2 6 Kt*A4*Ltt<^, 

(PR 2) l/--y^?f K2 2 0|SJ*«:IISLT^ L/-f h'-A2 4i<fi7 

SP^x TCP ©{IE 2 b left* * h *< JSfc S ft S , U--»f"\? K2 2 li* 

(P R 3 ) C ©ttSgTN o ** >y f>«&ISl^E 3 0 *^UA^ 72 1 IC«t £ ft 

z 0 sm$ft*:ftx#>y h ©ig«{iis«^a^K 2 o f*gT®««is$ n> a > 5 

2 1 ©77 y 7&g|3& (JH&Oc ) ^6flfc*X#y h (TCP&12bi:*5o ) 
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(PR4) <et >©x- * ItzZ, mmy-& 2 5 ^to 

u „, k 2 2 $ ttfc * * *£©T-s tit H 5 J; 5 lc> t- 

A2 4li^*TTCP (2b) *i5:ti:ttj 0 

(P R 5 ) 77 y 5ffiS^OJ1^0c ^blfcU-f t*-A2 4 © # & # ft £ 
^fc-T^^ h;i/<t Lt, V ;K eh > ©x- ? ^fPfiSc • IStift-T^o < eh > 
lix ifc© 2 h^T*5 0 

; et >tl/-ft'-A2 4*<»*¥ffiirtl-**o 

2 ;<eh >±<et >^ftf:t< eh > © *>521 *>&U-f 
^ y K22*fifc^|p](ciSV^©'<^ h^T*5 0 < eh > lis EI 1 3 \z 

Ell 3© (1) Ktk L*:«fc 9 ICT C P&B 2 b #ffi3 a ©±#fc * 
o *' y h ©ffi 3 a ^©giglcJ; EI 1 3 © (2) tc^Lfc«fc-5^TCP &S 2 
b#® 3 a ©T:£K#£#tJ!g£^So »f tixtfx X#-y h F©<&|f *<T C P& 
S (2 b) 415*^721 <DUm 2 1 a tM3 a©£;£N£i@x.TE!l 3© 

F©^ft^-^t)-r<^h^Kes >Of-^*^»5 0 K ? h JK e h >{i> C 
©<es >*ffl^T»:S (47) T'3»tB$tlSo 

<at>x <es> 

<eh>= • ♦ • (47) 

II < et>x < es> II 

(PR6) *>721<0 + t^l/-'>a ^Ik'nfi O o C tliix o ** -y h 
5fc© 7 5 > v»®±lctS:5£$ ntU 57v> '^11^ ( 2 a *<ICj&) i 77 * 5 
^©ISg&S-^SbS*:*!)©^©-?**., ?W 7©* + 'J7"l/-->a ^O^j&Col* 

2 0 



H#x.ffl^ (3UUI26) 
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[3] ZgCTTsftM 

(T H 1 ) jtf, o *' -y i? a ?"M K> T ft ft) <D £fc ^ ft M fit 3 * ft 

17 - * S 3 a ©±2f £ ■£> U-f t*-A2 40X^7 h F ^-ftS? - * 

5 ® 3 a±KJ&fi£f So tifittlAli, &j£-tr > V TBI RTItttftS 

7-?Tf»7-*>/LT*< tOiti, SI 1 lT^Lfcttl©77> < ;tt 
H2 a * g»^KHa^{&!l P siU ttIP s *&g*fFtt«Hf6*-« t©it 

10 011i:/T$tlTU5J;7i:, TCPffiI2 bliI3 a©±Ji:*^ U-^ 
t"-A24li:o^*ltI3a±i:^#'v hF So £ ©SFgT H 1 T 

l±. -ttlCx tfc^v-?A. Xtf? httBFSJ/TCP (2b) <D3&ttEl* 
KMOfirftK* * 0 £ ^ *^21 v- ? A £ jEfcSm 2 1 a li T 

CP (2b) $ft & ^ 0 

15 (T H 2 ) g^^iO^llCi: o T n # «y h ^r»bf^ ^ 1 & » 

? A^it^» 2 1a & TCP (2b) &fl£ <t r> lef 5 o n ** >y h © 
SjfttLTti. *y7lSIfflMOc ft ») <0®(i*<^aWT*So 

(T H 3 ) e^^ljOtoSJCfc o TO h ^ E!j iir . ft ft 2 1 a j&< T C 
P (2 b) *MZ>$kt¥Z V^K) h F © Jf$ J£ ft E £ tfc ^ £ 

20 t-^ Ai:gfi^t5o h©»jf££ Ltli, m» 2 1 a ^[SjOIJjiilcSS: 

(T H 4 ) m» 2 1a ^©HUft^ftSJ (C -a tz y> . SiH 2 1 a T C P 
(2 b) til* ftS&*jl2 1 a #|6] ic?& ^ T»il 

u ftA* y 7iiioi^oc ^ •? ome^ffi^-^^-tt So 

25 (TH5) KIT. TH3©»f* ( 113 ft) t T H 4 (ftft) ©»f£*« 0 ft L 

T, h F ©BfiSffift«-»^^^-^ A lCi&3iC£ -£ So C©JRs£^{i. TC 

P (2b) S-ftS . So. mum^-? A*14«I2 laiTCP (2b) £ 

ft5 b--tFtf-A©2Sj&fcffettS> S£oT N i^^litfc^^^ - ^ A, X 

# >v hffilFRtfTCP (2b) ©3 £*><|q| C ft» fc* 3 C t \Z t£ *> c c tl It t 
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o t>tt*ssfx tt^*fgij&fc'5T cp (2 b) i}<&mftm&.3Lfcmmitzz 

*C1T, C ©ttllgicfctt* n *' -y h ttH*ftiJ&#lfc (n**-y h EF*3 © ^ 

s * U ) icfHts-rntix aiAcaitii^Tts. gt^^sas (t* 

TH1-TH5 zmm-tnii&^o 

[4] ITjzS; 2 (Ci$l*Stf^»attRg*&ttB«<Z>&9 
10 ±E [2] TRWLfc^o-bxcjtot> l8J6ttfl P s (77^fi 

at'$D * £>tJc^*&M<4R p t crai:<, 7 5 > v'fitiiTsm) 

h ;i/ * ft t> -t t<, h ;u©ss*rt£*ii#s: l t (±sij®-e*> So s**i<e 

^ftlis Ell 5 (L 1~L 1 2) StfHl 6 (L13-L19) ®7n-f + - 

itikvm k> s» 

jfl , al ; ^fnfcn^y h©iI/Hii*Sfct7 7^-?±l©<l*i 
So + 1 ttl5Jt**«c - 1 So 
20 1 ; a*K«y h © flF JS/ftii © * *£ft t> V V X * fig 

<dm >;EU4lw7r:Lfc«fc?l^ # * v ffi&3fc©Hj& Oc $>|fc^£ v - 
? A*Jifcttli^lRl***>-i-i|lfit^^ (ft* 1 ) li«i7 ? -^^t>*26t» 

tl 5 o 

<ds > ; (3 C < EI 1 4 Liz J: -3 fcs # 5 &®&©Igj&0c £ x # 

25 », httSF (l/-1ffcf-A2 4©Afct&R) * £ tttt^Tffl * ft fc> * ? h 

;U (ft* 1) -C\ <dm >£HSU HRt*-**^**) &n* 0 

T ; a *' >y h M«&±T> n # -y h ©&R • £ 3$ * ft fc> t" fr W 

5r.<51 ; HftltlSS L t^if, 0 KiS l^IE© 5£»o 

R;<dm > £ < e t > © # JS] © M fl* £ IB & S Sliff ?'J-C R< e t > 
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= < dm >t'i53x 3f??iJ©rt> M.'\^<D Elte^g t> t h © . 



sgn (x) =+1 



(x > 0 ) 



sgn ( x ) = 0 



(x - 0 ) 



sgn ( x ) =-1 



(x < 0 ) 



3 cm) ic*7j»!f£^-f 3o 

CL 2] ftJijT Of - ^ ^H^t U ^ x ^ fi^ p 7 h©ISttSt:S^b 

CL 3 ] ^7^72 1 *ffl^T*K**fl^ H«toS^S 2 0 tm^T*? h 

(L 4) fcL> dn > J6 6*lttl;ntfx iSc*/&v - 9 A 77 

l 5 ^mtio 

(L 5) ^ * h ;K e t >i^n;Kdi >©^8<dr > £#«>So < 
dr > = < et >x< dm > 

(L6) ^n^Kdr > ©*fe*tfc£ L §V^i5r iJtKU /Jn $ < ttttft 
IfXr 7 7* L 7 ^iito tttU* Xt- 7"L10 ^iftfro 

(L 7) <dm >i<et > ®^|S]©I8^«- 12561" 5 laHEfi^J R *&IE©5£ 

R< et > = < di > ••• (48) 

R< dr > = < dr > ••• (49) 

(L8) *t*1T I/ 5>****, Iff L^SB^ff mRZ&tim 
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TgC = TC 



R 0 
t 



(5 0) 



0 1 

CL 9) o*** h 3r»f£3 -£x X7-v7L 8 T-ffW $ tliz'rt W T ©Sfet fit 

(LI 0) Xf-;^L6^ h ;K dr LlUfflfir «t»3 

lix £ > 5ffi«&©H;&0c httlBF *jafc«*2rfil*afc-f m<4^ 

? b;K ds >^xl!«7 f -^^t>*»5Ct^:K*^ 0 

(LID < ? h ;K d r > © Ife fcffe & J£ 0 ttioTUS C ©B#£ CT C 

£ *1 £ <> iSot, ^i'O^r-XTIi, ? h ;K d s > tf&tb £ tlx J* -r >y :7 L 

II3JJ^^It<£ -5) o 
(L 1 2) <ds >i< et >#flJlP IT, fio. F*3»l KttoT^ 
>y ? + Z (51 ti*« lc/Js $ ^iEffiT* 5. C i \z&M) „ 
( L 1 3 ) n ** » h CDfluiS/^jl*St)-r 7 7^al ^x <ds >i< eh > 
©rt«*<iEtt &tf (7 1 = - 1 x 6tfcrl = 1 > HKWlc 0 K h\ia \ = 0 t 

( L 1 4 ) ff 1 =0 tiMXr"; 7 L 1 5 Kilfro ct 1 = 0 Ttt litlli X 
t- v rf L l 6 ^ilC 

(L15) ct 1 *■ ct 0 IcKVr Lx Xx-y^L 1 S^iS^o 

(L16) ct 1 i ct 0 ©**<jET*ntf ^ x yfL 1 8 ~*jIko ^T&ntf 

(L 1 73 ctO £ct1 icMfr-TSit tic, 1 ©I**«t5, 
CL 1 8] fr?iJT ©-r- ? U ^x**l£x £IE©fr yijicft-f 4 fc© 

icMff-r^o C©?T?iJ{±x TtB©^^S; (51) *Tg l:ol>T»l, Tg -*T i 
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• • • (5 1) 

-y Zf L 1 2 T'C iXO^JBfftfci^Jri^iatlfcte £ . Ell 2© (TH5) ICTfiLtz 

&oK$i*ttm<iL!s.mmvimi><nmitztmi. n**.y h ^tfitstbTtos*** 

l:: Softie n *' -y h©ffc^:fcM&ig^©#t!i#^tt*>ftS©T% f^t!l«l^*<|S]± 
t5, 

(n**>y KftJfflIgffi3 0 &0'iI«#lSgH 2 0) ©rtSB©*aHT* 
HI 7 fix "^3 /SO©*- fe> g'^^ift©*- K^©tfJ&* •> x-r ArtS 

20 *Lfcto?*4. &x^«y ■r©^a«-ffl#^ie-r i^©«t 9 icttSo *{*J-et* 

t£$ nfc^5*N**iK $ nnwrxf- <y 7 s 2 icii*. $ fttt #tltf X^- 
^y S 1 gt. 

25 ( S 2 ] if a i/^t) > 4 7 Ttlg^nfcfKrtg CfffSK KISCT. 

BfrJS ©«!*«!» LTx n**«y h 1 0©^ 3 /SO l£!i*BB*&-f *o Wilix J 6 

tt + j*ip)©»»a<}&j£s tiT^n«\ j 6** + #iS]'^iga&sf*ibj&**ffcjifcL 

[S3] it 3 rig*) # ? > 4 7 & * 7 S ttfc gfr *«»r L * * 7 3 tltU* 

2 5 



TgC = T C 



d I < et> 
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xt- v -? s 8 Kit*, $ n^ynifxf-y •? s 4 iciitro 
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